#pragma config(Hubs,  S1, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, PIDControl, encoder)

task main()
{
  nMotorEncoder[motorD] = 0;
  nMotorEncoder[motorE] = 0;
  //nMotorEncoderTarget[motorD] = 5000;
  //nMotorEncoderTarget[motorE] = 5000;
 while(nMotorEncoder[motorD] < 6000)
 {
  motor[motorD] = 100;
  motor[motorE] = 100;
 }

 motor[motorD] = 0;
 motor[motorE] = 0;

  motor[motorD] = 75;
  wait1Msec(1000);

  nMotorEncoder[motorD] = 0; //initialize
  nMotorEncoder[motorE] = 0;
  motor[motorD] = 0;
  motor[motorE] = 0;

 while(nMotorEncoder[motorD] < 2000)
 {
  motor[motorD] = 100;
  motor[motorE] = 40;
 }

 motor[motorD] = 0;
 motor[motorE] = 0;


  motor[motorD] = 75;
  wait1Msec(1000);

  while(nMotorEncoder[motorD] < 1000)
 {
  motor[motorD] = 100;
  motor[motorE] = 100;
 }

  nMotorEncoder[motorD] = 0; //initialize
  nMotorEncoder[motorE] = 0;
  motor[motorD] = 0;
  motor[motorE] = 0;

 while(nMotorEncoder[motorD] < 6000)
 {
  motor[motorD] = 100;
  motor[motorE] = 100;
 }

}
